#include "../include/data_type_define.hpp"
// #include "mathcalls.h"
#include "math.h"

Data_Class::Data_Class()
{


}

void Data_Class::data_msg_whell_control(std::vector <int32_t> &msg)
{
  for(int i=0;i<8;i++) 
    speedset[i] = msg[i];
  for(int i=0;i<4;i++)
    angleset[i]=  msg[i+8];
  for(int i=0;i<8;i++)   
    motorother[i] = msg [i+8+4];
}
void Data_Class::data_info_wheel_msg(std::vector <int32_t> &msg)
{
    //msg.resize(30);
    for(int i=0;i<8;i++) 
      msg[i]=speedcur[i];
    for(int i=0;i<4;i++) 
      msg[i+8]=anglecur[i];  
    for(int i=0;i<4;i++) 
      msg[i+8+4]=yaogan[i];    
    for(int i=0;i<4;i++) 
      msg[i+8+4+4]=otherout[i];      
}
void Data_Class::data_msg_to_indata(std::vector <int32_t> &msg)
{
   for(int i=0;i<8;i++) 
      speedcur[i]=msg[i];
    for(int i=0;i<4;i++) 
      anglecur[i]=msg[i+8];  
    for(int i=0;i<4;i++) 
      yaogan[i]= msg[i+8+4];    
    for(int i=0;i<4;i++) 
      otherout[i]=msg[i+8+4+4];   
}
void Data_Class::data_control_to_msg(std::vector <int32_t> &msg)
{
     for(int i=0;i<8;i++) 
      msg[i]=speedset[i];
  for(int i=0;i<4;i++)
      msg[i+8]=angleset[i];
  for(int i=0;i<8;i++)   
     msg [i+8+4]=motorother[i] ;
}
// #define LUN_R_F 2
// #define LUN_R_B 3
// #define LUN_L_F 1
// #define LUN_L_B 0
void Data_Class::Feedback_cal(void)
{
  double fwhlspeed_rev[4];  //接收速度 左后 左前 右前 右后
  double fwhlspeed_abs[4];  //速度绝对值
  double fwhlangle_rev[4];  //接收角度
  double fwhlangle_act[4];  //实际角度 考虑反转 实际前进角度
  double vfrx,vfry,vflx;    //解算车整体速度所需三个速度
  double vx,vy,wz;          //所求整车 线速度xy方向 与 转动速度逆时针为正
  //根据反馈赋值
  int speed;
  for(int i=0;i<4;i++)
  {
    speed=get_module_speed_cur(i);
    fwhlspeed_rev[i] = convert_rmp_to_ms (speed);
    fwhlangle_rev[i] = double(anglecur[i] ) * (180.f / 16384.f);
  }
  //给所有数组数据赋值

  for( int i = 0 ; i < 4 ; i++ )
  {
    //fwhlspeed_rev[i] = 需要速度值 除以？
    fwhlspeed_abs[i] = abs(fwhlspeed_rev[i]);
    //fwhlangle_rev[i] = 需要角度值° 除以16384 
    if(fwhlspeed_rev[i] >= 0)
    {
      fwhlangle_act[i] = fwhlangle_rev[i];
    }
    else
    {
      if(fwhlangle_rev[i] >= 0)
      {
        fwhlangle_act[i] = fwhlangle_rev[i] - 180.0;
      }
      else
      {
        fwhlangle_act[i] = fwhlangle_rev[i] + 180.0;
      }   

    }

    //分解得到三个速度
    vfrx = fwhlspeed_abs[2] * cos(fwhlangle_act[2] * M_PI / 180.0);
    vfry = fwhlspeed_abs[2] * sin(fwhlangle_act[2] * M_PI / 180.0);
    vflx = fwhlspeed_abs[1] * cos(fwhlangle_act[1] * M_PI / 180.0);

    //求解整车速度
    vx = ( vfrx + vfry ) * 0.5; 
    wz = ( vfrx - vfry ) / ( 2.0 * RX );
    vy = vfry -wz *RY;

    /////////
    cur_vx =vx; 
    cur_vy =vy;
    cur_wz =wz;
  }
  

}
void Data_Class::Driver_cal(void)
{
   int nov_flag;
  //驱动算法矩阵B=AX，计算结果为矢量                 
  double vfr_x = vx + RX * wz;
  double vfr_y = vy + RY * wz;
  double vfl_x = vx - RX * wz;
  double vfl_y = vy + RY * wz;
  double vrl_x = vx - RX * wz;
  double vrl_y = vy - RY * wz;
  double vrr_x = vx + RX * wz;
  double vrr_y = vy - RY * wz;

  //矢量分解，得到速度值和角度                    
  // angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  // angle_c[2] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  // angle_c[0] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  // angle_c[3] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  // rpm_c[1] = calcDistance(vfl_x, vfl_y);
  // rpm_c[2] = calcDistance(vfr_x, vfr_y);
  // rpm_c[0] = calcDistance(vrl_x, vrl_y);
  // rpm_c[3] = calcDistance(vrr_x, vrr_y);
  angle_c[2] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  angle_c[1] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  angle_c[3] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  angle_c[0] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  rpm_c[2] = calcDistance(vfl_x, vfl_y);
  rpm_c[1] = calcDistance(vfr_x, vfr_y);
  rpm_c[3] = calcDistance(vrl_x, vrl_y);
  rpm_c[0] = calcDistance(vrr_x, vrr_y);
  //最合适执行的角度与速度
  for( int i = 0; i < 4; i++ )
  {
    //速度上限
    if( rpm_c[i] >= 128 )
    {
      rpm_c[i] = 128;
    }

    //判断是否需要反转
    if( ( angle_c[i] >= 90.0 ) || ( angle_c[i] <= -90.0 ) )
    {
      nov_flag = 1;
    }
    else
    {
      nov_flag = 0;
    }

    //得到最合适角度与速度
    if( nov_flag == 1 )
    {
      angle_b[i] = angle_c[i] - ( sgn(angle_c[i]) * 180.0 );
      rpm_b[i]   = -rpm_c[i];
    }
    else
    {
      angle_b[i] = angle_c[i];
      rpm_b[i]   = rpm_c[i];
    }

  }
  
}
int Data_Class::Driver_tiao(void)
{
  int tiao_c=0;
  if(wz==0)return tiao_c;
  double x,max;
  int nob;
  max=0;
   if(vx!=0) //左右拐弯
   {
      for(int i=0;i<4;i++)
      {
      x=angle_n[i]-angle_b[i];
      if(max<abs(x))
        {
          max=x; nob=i;
          
          }
      }
      x=angle_n[nob]-angle_b[nob];
          angle_b[nob]=angle_b[nob]-x*0.36;
      
   }
//    #define LUN_R_F 2
// #define LUN_R_B 3
// #define LUN_L_F 1
// #define LUN_L_B 0
   else if(wz!=0) //原地转 360度
    {
      if(wz<0)
        {
        angle_b[1]=angle_b[LUN_L_F]-6.5;
        angle_b[3]=angle_b[LUN_R_B]-6.5;
          angle_b[0]=angle_b[LUN_L_B]-3.5;
        angle_b[2]=angle_b[LUN_R_F]-3.5;
        }
        if(wz>0)
        {
        angle_b[1]=angle_b[LUN_L_F]+10.0;      //15.0;
        angle_b[3]=angle_b[LUN_R_B]+10.0;
        angle_b[0]=angle_b[LUN_L_B]+3.5;
        angle_b[2]=angle_b[LUN_R_F]+3.5;
        }
    //   for(int i=0;i<4;i++)
    //   {
         
    //          if(angle_n[i]<angle_b[i])
    //             {
    //                if(angle_shift[i]<0.0)angle_shift[i]=0.0;
    //               angle_shift[i]+=0.08;
    //               angle_b[i]=angle_b[i]+angle_shift[i];
    //             }
    //          else 
    //           {
    //               if(angle_shift[i]>0.0)angle_shift[i]=0.0;
    //               angle_shift[i]+=-0.08;
    //               angle_b[i]=angle_b[i]+angle_shift[i]; 
    //           }   
       
    //    }
      
     }
    // else if(wz==0)for(int i=0;i<4;i++)   angle_shift[i]=0.0;
  return tiao_c;
}

void Data_Class::yaokong(void)
{
  int wz_new;
  vx=yaogan[1];
  wz_new=yaogan[2];
  std::cout<< "vx= "<<vx<< " wz="<<wz_new<<std::endl;
  //限制拐弯速度
    if(vx!=0)
      {
       if((wz_new<20)&&(wz_new>-20))
            { if(max_speed < LINE_MAX_SPEED) max_speed +=10; if(max_speed > LINE_MAX_SPEED)max_speed = LINE_MAX_SPEED;}
         else 
            { if(max_speed > CIRCLE_MAX_SPEED) max_speed -=10; if(max_speed < CIRCLE_MAX_SPEED)max_speed = CIRCLE_MAX_SPEED;}
         
      }
    wz=wz_new;  
    // angle_n[0] = double(anglecur[0] ) * (-180.f / 16384.f); 
    // angle_n[1] = double(anglecur[1] ) * (-180.f / 16384.f); 
    // angle_n[2] = double(anglecur[2] ) * (-180.f / 16384.f); 
    // angle_n[3] = double(anglecur[3] ) * (-180.f / 16384.f);  
    angle_n[0] = double(anglecur[0] ) * (180.f / 16384.f); 
    angle_n[1] = double(anglecur[1] ) * (180.f / 16384.f); 
    angle_n[2] = double(anglecur[2] ) * (180.f / 16384.f); 
    angle_n[3] = double(anglecur[3] ) * (180.f / 16384.f);  
    for(int i=0;i<4;i++)
    {
        std::cout<< "anglecur"<<i<< "="<<anglecur[i]<<"  angle_n="<<angle_n[i]<<std::endl;
    }
     std::cout<< "max_speed=" <<max_speed <<std::endl;
//驱动算法，得四个速度与四个角度
     Driver_cal();
     Driver_tiao();    //角度过调整
     speedset[0] = rpm_b[0] * ( max_speed / 128.f);    //                    
     speedset[1] = rpm_b[1] * ( max_speed / 128.f);    //
     speedset[2] = rpm_b[2] * ( max_speed / 128.f);    //
     speedset[3] = rpm_b[3] * ( max_speed / 128.f);    //

    angleset[0] = angle_b[0] * ( 16384.f / 180.f );
    angleset[1] = angle_b[1] * ( 16384.f / 180.f );
    angleset[2] = angle_b[2] * ( 16384.f / 180.f );
    angleset[3] = angle_b[3] * ( 16384.f / 180.f );

}
float Data_Class::convert_rmp_to_ms(int sp)
{
   float spd,spf;
   spf=sp;
   spd= WHEEL_RADIUS*M_PI*2.0*spf/(50.0*60.0);
   return spd;
}
int Data_Class::convert_ms_to_rmp(float fsp)
{
  float sp;
  int spd;
  sp=fsp*60.0/((WHEEL_RADIUS*M_PI*2.0)/50.0);
  spd=(int)sp;
  return spd;
}
float Data_Class::convert_yaogan_wmga( int16_t w)
{
  float yg,wmga;
  yg=(float)w;
  wmga= wmga_max *yg/128.0;
  return wmga;
}
float Data_Class::convert_yaogan_line(int16_t v)
{
  float vt,tempf;
  tempf =(float) v;
  vt= v /128.0;
  return vt;
}

void Data_Class::yaokong_real(float linex,float wmga,float l_y)
{
  float line,wmg;
  // line= convert_yaogan_line(yaogan[1]);
  // wmg = convert_yaogan_wmga(yaogan[2]);
 line=linex; wmg=wmga;
//  if((my_abs(linex)<0.02)&&(my_abs(wmga)<0.02)&&(my_abs(l_y)<0.02))
//     all_stop_flag=1;
//  else  all_stop_flag=0;  
 // std::cout<<"yg1="<<yaogan[1]<<" yg2="<<yaogan[2]<<std::endl;
  // std::cout<<"yg1="<<line<<" yg2="<<wmg<<std::endl;
  // std::cout<<"v0="<< rpm_b [0]<<"v1="<< rpm_b [1]<<"v2="<< rpm_b [2]
  //               <<"v3="<< rpm_b [3]<<std::endl;
  // std::cout<<"a0="<< angle_b [0]<<"a1="<< angle_b [1]<<"a2="<< angle_b [2]
  //               <<"a3="<< angle_b [3]<<std::endl;
    angle_n[0] = double(anglecur[0] ) * (180.f / 16384.f); 
    angle_n[1] = double(anglecur[1] ) * (180.f / 16384.f); 
    angle_n[2] = double(anglecur[2] ) * (180.f / 16384.f); 
    angle_n[3] = double(anglecur[3] ) * (180.f / 16384.f);  
   // for(int i=0;i<4;i++)
      
    //驱动算法，得四个速度与四个角度
   //  Driver_cal();
   //  Driver_tiao();    //角度过调整
    Driver_cal_real(line,wmg,l_y);
     Driver_tiao();    //角度过调整
    
     speedset[0] =convert_ms_to_rmp( rpm_b[0]);  //        
    speedset[1] =convert_ms_to_rmp( rpm_b[1] );  //      
    speedset[2] =convert_ms_to_rmp( rpm_b[2] );  //      
    speedset[3] =convert_ms_to_rmp( rpm_b[3] );  //      
    

    angleset[0] = angle_b[0] * ( 16384.f / 180.f );
    angleset[1] = angle_b[1] * ( 16384.f / 180.f );
    angleset[2] = angle_b[2] * ( 16384.f / 180.f );
    angleset[3] = angle_b[3] * ( 16384.f / 180.f );
}
void Data_Class::yaokong_real(float linex,float wmga)
{
  float line,wmg;
  line= convert_yaogan_line(yaogan[1]);
  wmg = convert_yaogan_wmga(yaogan[2]);
 
 // std::cout<<"yg1="<<yaogan[1]<<" yg2="<<yaogan[2]<<std::endl;
  // std::cout<<"yg1="<<line<<" yg2="<<wmg<<std::endl;
  std::cout<<"v0="<< rpm_b [0]<<"v1="<< rpm_b [1]<<"v2="<< rpm_b [2]
                <<"v3="<< rpm_b [3]<<std::endl;
  std::cout<<"a0="<< angle_b [0]<<"a1="<< angle_b [1]<<"a2="<< angle_b [2]
                <<"a3="<< angle_b [3]<<std::endl;
    angle_n[0] = double(anglecur[0] ) * (180.f / 16384.f); 
    angle_n[1] = double(anglecur[1] ) * (180.f / 16384.f); 
    angle_n[2] = double(anglecur[2] ) * (180.f / 16384.f); 
    angle_n[3] = double(anglecur[3] ) * (180.f / 16384.f);  
   // for(int i=0;i<4;i++)
      
    //驱动算法，得四个速度与四个角度
   //  Driver_cal();
   //  Driver_tiao();    //角度过调整
    Driver_cal_real(line,wmg);
     Driver_tiao();    //角度过调整
     speedset[0] =convert_ms_to_rmp( rpm_b[0]);  //        
    speedset[1] =convert_ms_to_rmp( rpm_b[1] );  //      
    speedset[2] =convert_ms_to_rmp( rpm_b[2] );  //      
    speedset[3] =convert_ms_to_rmp( rpm_b[3] );  //      
    

    angleset[0] = angle_b[0] * ( 16384.f / 180.f );
    angleset[1] = angle_b[1] * ( 16384.f / 180.f );
    angleset[2] = angle_b[2] * ( 16384.f / 180.f );
    angleset[3] = angle_b[3] * ( 16384.f / 180.f );
}
void Data_Class::Driver_cal_real(float line,float wmga,float l_y)
{
   int nov_flag;
   float liney=l_y;
   float linex=0;
   if(wmga>0.6)wmga=0.6;
   if(wmga<-0.6)wmga=-0.6;
   linex=line;        //+0.001;
   
  //驱动算法矩阵B=AX，计算结果为矢量                 
  double vfr_x = linex + RX * wmga;
  double vfr_y = liney + RY * wmga;
  double vfl_x = linex - RX * wmga;
  double vfl_y = liney + RY * wmga;
  double vrl_x = linex - RX * wmga;
  double vrl_y = liney - RY * wmga;
  double vrr_x = linex + RX * wmga;
  double vrr_y = liney - RY * wmga;

  //矢量分解，得到速度值和角度                    
  // angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  // angle_c[2] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  // angle_c[0] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  // angle_c[3] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  // rpm_c[1] = calcDistance(vfl_x, vfl_y);
  // rpm_c[2] = calcDistance(vfr_x, vfr_y);
  // rpm_c[0] = calcDistance(vrl_x, vrl_y);
  // rpm_c[3] = calcDistance(vrr_x, vrr_y);
  angle_c[2] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  angle_c[1] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  angle_c[3] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  angle_c[0] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  rpm_c[2] = calcDistance(vfl_x, vfl_y);
  rpm_c[1] = calcDistance(vfr_x, vfr_y);
  rpm_c[3] = calcDistance(vrl_x, vrl_y);
  rpm_c[0] = calcDistance(vrr_x, vrr_y);
  //最合适执行的角度与速度
  for( int i = 0; i < 4; i++ )
  {
    //速度上限
    if( rpm_c[i] >= 128 )
    {
      rpm_c[i] = 128;
    }

    //判断是否需要反转
    if( ( angle_c[i] >= 90.0 ) || ( angle_c[i] <= -90.0 ) )
    {
      nov_flag = 1;
    }
    else
    {
      nov_flag = 0;
    }

    //得到最合适角度与速度
    if( nov_flag == 1 )
    {
      angle_b[i] = angle_c[i] - ( sgn(angle_c[i]) * 180.0 );
      rpm_b[i]   = -rpm_c[i];
    }
    else
    {
      angle_b[i] = angle_c[i];
      rpm_b[i]   = rpm_c[i];
    }

  }
  
}
void Data_Class::Driver_cal_real(float line,float wmga)
{
   int nov_flag;
   float liney=0.0;
   float linex=0;
   if(wmga>0.6)wmga=0.6;
   if(wmga<-0.6)wmga=-0.6;
   linex=line;        //+0.001;
  //驱动算法矩阵B=AX，计算结果为矢量                 
  double vfr_x = linex + RX * wmga;
  double vfr_y = liney + RY * wmga;
  double vfl_x = linex - RX * wmga;
  double vfl_y = liney + RY * wmga;
  double vrl_x = linex - RX * wmga;
  double vrl_y = liney - RY * wmga;
  double vrr_x = linex + RX * wmga;
  double vrr_y = liney - RY * wmga;

  //矢量分解，得到速度值和角度                    
  // angle_c[1] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  // angle_c[2] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  // angle_c[0] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  // angle_c[3] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  // rpm_c[1] = calcDistance(vfl_x, vfl_y);
  // rpm_c[2] = calcDistance(vfr_x, vfr_y);
  // rpm_c[0] = calcDistance(vrl_x, vrl_y);
  // rpm_c[3] = calcDistance(vrr_x, vrr_y);
  angle_c[2] = atan2(vfl_y, vfl_x) * 180 / M_PI; 
  angle_c[1] = atan2(vfr_y, vfr_x) * 180 / M_PI;
  angle_c[3] = atan2(vrl_y, vrl_x) * 180 / M_PI;
  angle_c[0] = atan2(vrr_y, vrr_x) * 180 / M_PI;

  rpm_c[2] = calcDistance(vfl_x, vfl_y);
  rpm_c[1] = calcDistance(vfr_x, vfr_y);
  rpm_c[3] = calcDistance(vrl_x, vrl_y);
  rpm_c[0] = calcDistance(vrr_x, vrr_y);
  //最合适执行的角度与速度
  for( int i = 0; i < 4; i++ )
  {
    //速度上限
    if( rpm_c[i] >= 128 )
    {
      rpm_c[i] = 128;
    }

    //判断是否需要反转
    if( ( angle_c[i] >= 90.0 ) || ( angle_c[i] <= -90.0 ) )
    {
      nov_flag = 1;
    }
    else
    {
      nov_flag = 0;
    }

    //得到最合适角度与速度
    if( nov_flag == 1 )
    {
      angle_b[i] = angle_c[i] - ( sgn(angle_c[i]) * 180.0 );
      rpm_b[i]   = -rpm_c[i];
    }
    else
    {
      angle_b[i] = angle_c[i];
      rpm_b[i]   = rpm_c[i];
    }

  }
  
}
int Data_Class::get_module_speed_cur(int nob)
{
  int sp1,sp2;
  sp1=-speedcur[nob*2];
  sp2= speedcur[nob*2+1];
  return (sp1+sp2)/2;
}
